
//
// Created by deng on 2022/12/25.
//
#include <cstring>
#include <cmath>
#include "common_inc.h"
#include "configurations.h"

extern EncoderCalibrator encoderCalibrator;

extern Motor motor;

extern bool DebugPidvFlag;
extern uint32_t DebugPidvDelMs;

bool IfTofData(uint8_t *p){
    if(p[0] == 0x55 && p[1] == 0xAA && p[2] == 0x81 && p[p[3] + 5 - 1] == 0xFA)
        return true;
    else
        return false;
}

void OnUartCmd(uint8_t* _data, uint16_t _len)
{
    static float cur, pos, vel, time;
    static int ret = 0;

    if(IfTofData(_data)){
        TofDis = ((uint32_t)_data[4]<<8) + _data[5];
        _data[0] = 0;
        _data[1] = 0;
    }else{
        switch (_data[0])
        {
            case 'a':
                encoderCalibrator.isTriggered = true;
                break;
            case 'c':
                ret = sscanf((char*) _data, "c %f", &cur);
                if (ret < 1)
                {
                    printf("[error] Command format error!\r\n");
                } else if (ret == 1)
                {
                    if (motor.controller->modeRunning != Motor::MODE_COMMAND_CURRENT)
                        motor.controller->SetCtrlMode(Motor::MODE_COMMAND_CURRENT);
                    motor.controller->SetCurrentSetPoint((int32_t) (cur * 1000));
                }
                break;
            case 'v':
                ret = sscanf((char*) _data, "v %f", &vel);
                if (ret < 1)
                {
                    printf("[error] Command format error!\r\n");
                } else if (ret == 1)
                {
                    if (motor.controller->modeRunning != Motor::MODE_COMMAND_VELOCITY)
                    {
                        motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                        motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);
                    }
                    motor.controller->SetVelocitySetPoint(
                            (int32_t) (vel * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));
                }
                break;
            case 'p':
                float speed;
                ret = sscanf((char*) _data, "p %f,%f", &pos,&speed);
                printf("pos:%f,speed:%f\n",pos,speed);
                if (ret < 2)
                {
                    printf("[error] Command format error!\r\n");
                } else if (ret == 2)
                {
                    if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
                        motor.controller->requestMode = Motor::MODE_COMMAND_POSITION;

                    motor.config.motionParams.ratedVelocity =
                            (int32_t) (speed * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);

                    motor.controller->SetPositionSetPoint(
                            (int32_t) (pos * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS));

//                    if( motor.controller->requestMode == Motor::MODE_COMMAND_POSITION){
//                        motor.controller->requestMode = Motor::MODE_STOP;
//                    }
                }
                break;
            case '$':
                if(strstr((char*)_data,"cmd ") != NULL) {
                    if (strstr((char *) _data, " set ID:") != NULL) {
                        int id;
                        ret = sscanf((char *) _data, "$cmd set ID:%d", &id);
                        if (ret == 1 && id > 0 && id <= 6) {
                            boardConfig.canNodeId = id;
                            printf("id set to %d\n", id);
                            boardConfig.configStatus = CONFIG_COMMIT;
                        }
                    } else if (strstr((char *) _data, " set HardVer:") != NULL) {
                        int HardVer;
                        ret = sscanf((char *) _data, "$cmd set HardVer:%d", &HardVer);
                        if (ret == 1 && HardVer > 0 && HardVer < HARD_VER_SUM_IN_THIS_SOFT_VER) {
                            boardConfig.HardVer = (HARD_VER) HardVer;
                            printf("HardVer set to %d\n", HardVer);
                            boardConfig.configStatus = CONFIG_COMMIT;
                        }
                    } else if (strstr((char *) _data, " set ACC:") != NULL) {
                        float val;
                        ret = sscanf((char *) _data, "$cmd set ACC:%f", &val);
                        if (ret == 1 && val > 0 && val <= 40) {
                            motor.config.motionParams.ratedVelocityAcc =
                                    50 * val * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                            motor.motionPlanner.velocityTracker.SetVelocityAcc(
                                    motor.config.motionParams.ratedVelocityAcc);
                            motor.motionPlanner.positionTracker.SetVelocityAcc(
                                    motor.config.motionParams.ratedVelocityAcc);
                            printf("ACC set to %f\n", val);
                        }
                    } else if (strstr((char *) _data, " find home ") != NULL) {
                        int Type, UpdataFlag;
                        float v;
                        ret = sscanf((char *) _data, "$cmd find home Speed:%f Type:%d Updata:%d", &v, &Type,
                                     &UpdataFlag);
                        if (ret == 3 && FindedHomeFlag == false && fabs(v) <= 10 &&
                            (Type == FIND_HOME_WITH_PHYSICAL_LIMIT || Type == FIND_HOME_WITH_PHYSICAL_KEY)) {
                            printf("ret:%d find home Speed:%f Type:%d Updata:%d", ret, v, Type, UpdataFlag);
                            motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
                            motor.controller->SetCtrlMode(Motor::MODE_COMMAND_VELOCITY);

                            motor.controller->SetVelocitySetPoint(
                                    (int32_t) (v *
                                               (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS) *
                                    (IsCalibratorKeyOn() == false ? 1 : -1));


                            if (UpdataFlag == 1)
                                FindHomeFlagUpdataFlag = true;
                            else
                                FindHomeFlagUpdataFlag = false;
                            motor.config.motionParams.ratedCurrent = (int32_t) 10 * 100;

                            if (IsCalibratorKeyOn() == false)
                                NeedFindHomeFalg = (NEED_FIND_HOME_FLAG) Type;
                        }
                    } else if (strstr((char *) _data, " set PIDV:") != NULL) {
                        float val;
                        ret = sscanf((char *) _data, "$cmd set PIDV:%d,%d,%d,%d", &motor.config.ctrlParams.dce.kp,
                                     &motor.config.ctrlParams.dce.ki, &motor.config.ctrlParams.dce.kd,
                                     &motor.config.ctrlParams.dce.kv);
                        printf("PIDV set to %d,%d,%d,%d\n", motor.config.ctrlParams.dce.kp,
                               motor.config.ctrlParams.dce.ki, motor.config.ctrlParams.dce.kd,
                               motor.config.ctrlParams.dce.kv);

                    }else if (strstr((char *) _data, "debug pidv del:") != NULL) {
                        uint32_t val;
                        ret = sscanf((char *) _data, "$cmd debug pidv del:%d", &val);
                        if (ret == 1 && val >= 1 && val <= 10000) {
                            DebugPidvFlag = true;
                            DebugPidvDelMs = val;
                            printf("debug pidv del set to %d ms\n", DebugPidvDelMs);
                        }
                    }
                }if(strstr((char*)_data,"M [J] ") != NULL) {
                    encoderCalibrator.isTriggered = true;
                }
                else if (HARD_VER_INIT == MR6 && boardConfig.canNodeId == 6 || HARD_VER_INIT == BATTLE_CHARGE_THERE_AXIS && boardConfig.canNodeId == 3) {
                    TofDis = 0xFFFFFFFE;
                }

                break;
        }
    }


}

